/**
 * @file Robot.h
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief 机器人类
 */

#ifndef __ROBOT_H__
#define __ROBOT_H__

#include "Coord.h"
#include "Vector.h"
#include "PIDController.h"

/**
 * @brief 机器人
 */
class Robot: public Coord {
private:
    int id; // 机器人ID
    int benchID; // 所处工作台ID
    int materialType; // 携带物品类型
    double timeFactor; // 时间价值系数
    double crashFactor; // 碰撞价值系数
    double angleSpeed; // 角速度
    Vector lineSpeed; // 线速度
    double yaw; // 当前航向
    PIDParam_t forwardEmpty; // 空载时的前进PID参数
    PIDParam_t forwardFull; // 满载时的前进PID参数
    PIDParam_t rotateEmpty; // 空载时的旋转PID参数
    PIDParam_t rotateFull; // 满载时的旋转PID参数
    const static double EMPTY_RADIUS; // 空载半径
    const static double FULL_RADIUS; // 满载半径
    double radius = EMPTY_RADIUS; // 半径
    Coord coordTarget; // 目标坐标
    bool isRun = false; // 是否为移动任务
    bool isCtrling = false; // 是否正在执行运动控制
    bool needWaitRotate = false; // 是否需要等待旋转控制完成再起跑

public:
    PIDController forwardPID; // 前进PID
    AnglePIDController rotatePID; // 旋转PID

private:
    double getRotate();
    double getForward(double minForward = ROBOT_FORWARD_MIN, double maxForward = ROBOT_FORWARD_MAX);

public:
    void init(int id, int x, int y);
    void refresh(int benchID, int materialType, double timeFactor, double crashFactor, double angleSpeed,
                 double lineSpeed_x, double lineSpeed_y, double yaw, double coord_x, double coord_y);
    void initPID(PIDParam_t forwardEmpty, PIDParam_t forwardFull, PIDParam_t rotateEmpty, PIDParam_t rotateFull);
    void forward(double speed);
    void rotate(double speed);
    void buy();
    void sell();
    void destroy();
    double getRadius();
    void turnTo(Coord target);
    void turnTo(double x, double y);
    void runTo(Coord target, bool needWaitRotate = false);
    void runTo(double x, double y, bool needWaitRotate = false);
    void stopRotateCtrl();
    void stopCtrl();
    bool isFinishedCtrl();
    void sendCtrlCommand();
    int getMaterialType();
    int getID();
    bool isEmpty();
    int getBenchID();
    static void pauseAll();
};

#endif // __ROBOT_H__
